
#ifndef __MLS_AFFINE_H__
#define __MLS_AFFINE_H__

#include "mlsRaw.h"

void momentumMatrix(const float p[], const float pstar[], const vector<Point3> &P, 
                    float M0[], float M1[], float M2[]) {
   
   const unsigned int NCTRLPOINTS = P.size();
   float pjhat[3];
   M0[0]=M0[1]=M0[2]=0.0;
   M1[0]=M1[1]=M1[2]=0.0;
   M2[0]=M2[1]=M2[2]=0.0;
   for (unsigned int j=0; j<NCTRLPOINTS; j++) {
      float W = weight(p,P[j]);
      pjhat[0] = P[j][0]-pstar[0];  pjhat[1] = P[j][1]-pstar[1]; pjhat[2] = P[j][2]-pstar[2];
      M0[0] += W*pjhat[0]*pjhat[0]; M0[1] += W*pjhat[0]*pjhat[1]; M0[2] += W*pjhat[0]*pjhat[2];
      M1[0] += W*pjhat[1]*pjhat[0]; M1[1] += W*pjhat[1]*pjhat[1]; M1[2] += W*pjhat[1]*pjhat[2];
      M2[0] += W*pjhat[2]*pjhat[0]; M2[1] += W*pjhat[2]*pjhat[1]; M2[2] += W*pjhat[2]*pjhat[2];
   }
}

void mlsAffineEval(const vector<Point3> &P, const vector<Point3> &Q, float p[], float pd[]) {
   
   /// compute weighted centroids
   float pstar[3], qstar[3];
   weightedCentroids(p, pstar, qstar, P, Q);
   
   /// compute the momentum matrix
   float A0[3], A1[3], A2[3]; // the matrix rows
   momentumMatrix(p, pstar, P, A0, A1, A2);
   
   /// Compute the inverse of the momentum matrix
   float Ai[3][3];
   Ai[0][0]=  A2[2]*A1[1]-A2[1]*A1[2];  Ai[0][1]=-(A2[2]*A0[1]-A2[1]*A0[2]); Ai[0][2]=  A1[2]*A0[1]-A1[1]*A0[2];
   Ai[1][0]=-(A2[2]*A1[0]-A2[0]*A1[2]); Ai[1][1]=  A2[2]*A0[0]-A2[0]*A0[2];  Ai[1][2]=-(A1[2]*A0[0]-A1[0]*A0[2]);
   Ai[2][0]=  A2[1]*A1[0]-A2[0]*A1[1];  Ai[2][1]=-(A2[1]*A0[0]-A2[0]*A0[1]); Ai[2][2]=  A1[1]*A0[0]-A1[0]*A0[1];
   float detA = A0[0]*(A2[2]*A1[1]-A2[1]*A1[2])
               -A1[0]*(A2[2]*A0[1]-A2[1]*A0[2])
               +A2[0]*(A1[2]*A0[1]-A1[1]*A0[2]);
   Ai[0][0] /= detA;  Ai[0][1] /= detA; Ai[0][2] /= detA;
   Ai[1][0] /= detA;  Ai[1][1] /= detA; Ai[1][2] /= detA;
   Ai[2][0] /= detA;  Ai[2][1] /= detA; Ai[2][2] /= detA;
   
   /// compute the correlation matrix
   float M0[3], M1[3], M2[3]; // the matrix rows
   float dotA;
   correlationMatrix(p, pstar, qstar, P, Q, M0, M1, M2, dotA);
   float M[3][3], C[3][3];
   M[0][0]=M0[0]; M[0][1]=M0[1]; M[0][2]=M0[2];
   M[1][0]=M1[0]; M[1][1]=M1[1]; M[1][2]=M1[2];
   M[2][0]=M2[0]; M[2][1]=M2[1]; M[2][2]=M2[2];
   
   float Mt[3][3];
   Mt[0][0]=M[0][0]; Mt[0][1]=M[1][0]; Mt[0][2]=M[2][0];
   Mt[1][0]=M[0][1]; Mt[1][1]=M[1][1]; Mt[1][2]=M[2][1];
   Mt[2][0]=M[0][2]; Mt[2][1]=M[1][2]; Mt[2][2]=M[2][2];
   
   multiply(Ai,Mt,C);
   //~ multiply(Ai,M,C);
   
   /// modify the vertex position
   float vhat[3] = {p[0]-pstar[0], p[1]-pstar[1], p[2]-pstar[2]};
   float vx = vhat[0]*C[0][0] + vhat[1]*C[1][0] + vhat[2]*C[2][0];
   float vy = vhat[0]*C[0][1] + vhat[1]*C[1][1] + vhat[2]*C[2][1];
   float vz = vhat[0]*C[0][2] + vhat[1]*C[1][2] + vhat[2]*C[2][2];
   pd[0] = qstar[0] + vx;
   pd[1] = qstar[1] + vy;
   pd[2] = qstar[2] + vz;
}

#endif
